﻿using System;
using System.Linq;
using System.Collections.Generic;
using System.Text;
using WiMo.Games.Drawables;
using WiMo.Games.WM5.Drawables;
using System.Drawing;

namespace WiMo.Games.WM5.Physics
{
    public class CollisionUtils
    {
        public static Vector2 GetDistance(ISprite sprite1, ISprite sprite2)
        {
            Vector2 delta;
            Vector2 nDelta = new Vector2();
            var intersection = new Point();

            switch (sprite1.CollisionShape)
            {
                case CollisionShapes.LineList:
                    switch (sprite2.CollisionShape)
                    {
                        case CollisionShapes.LineList:
                            throw new NotImplementedException();
                        case CollisionShapes.Rectangle:
                            break;
                        case CollisionShapes.Sphere:
                            break;
                        case CollisionShapes.SphereList:
                            throw new NotImplementedException();
                    }
                    break;
                case CollisionShapes.Rectangle:
                    switch (sprite2.CollisionShape)
                    {
                        case CollisionShapes.LineList:
                            for (var idx = 0; idx < sprite2.CollisionList.Count - 1; ++idx)
                            {
                                                                
                            }
                            
                            break;
                        case CollisionShapes.Rectangle:
                        case CollisionShapes.Sphere:
                            delta = MathHelper.Distance(sprite1.Position, sprite2.Position);
                            nDelta = delta.Clone();
                            nDelta.Abs();
                            break;
                        case CollisionShapes.SphereList:
                            throw new NotImplementedException();
                    }
                    break;
                case CollisionShapes.Sphere:
                    switch (sprite2.CollisionShape)
                    {
                        case CollisionShapes.LineList:
                            break;
                        case CollisionShapes.Rectangle:
                        case CollisionShapes.Sphere:
                            delta = MathHelper.Distance(sprite1.Position, sprite2.Position);
                            nDelta = delta.Clone();
                            nDelta.Abs();

                            break;
                        case CollisionShapes.SphereList:
                            throw new NotImplementedException();
                    }
                    break;
                case CollisionShapes.SphereList:
                    throw new NotImplementedException();
                    //switch (sprite2.CollisionShape)
                    //{
                    //    case CollisionShapes.LineList:
                    //        break;
                    //    case CollisionShapes.Rectangle:
                    //        break;
                    //    case CollisionShapes.Sphere:
                    //        break;
                    //    case CollisionShapes.SphereList:
                    //        break;
                    //}
            }


            return nDelta;
        }

        private static bool Intersects(PointF spherePosition, int radius, int x, int y)
        {
            var delta = new Vector2(spherePosition.X - x, spherePosition.Y - y);
            return delta.Magnitude < radius;
        }

        public static bool RectToSphereCollisionDetection(ISprite rect1, ISprite sphere1)
        {
            var offset = sphere1.Position;
            if (sphere1.Origin == Origin.Center)
            {
                
                offset.X += sphere1.Size.Width / 2;
                offset.Y += sphere1.Size.Height / 2;
            }

            foreach(var point in sphere1.CollisionDetectionPoints)
            {
                var point1 = point.Add(offset);

                if (rect1.BoundingRect.Contains(point1))
                    return true;
            }

            return false;
        }

        public static bool RectToRectCollisionDetection(ISprite rect1, ISprite rect2)
        {
            return (rect1.BoundingRect.IntersectsWith(rect2.BoundingRect));
        }

        public static bool RectToLineListCollisionDetection(ISprite rect1, ISprite lineList, ref Vector2 intersection)
        {
            var spriteShape = lineList as ISpriteShape;

            if (spriteShape == null)
                throw new Exception("Invalid object type");

            for (var idx = 0; idx < spriteShape.Points.Count - 1; ++idx)
            {
                if (FindIntersection(
                    spriteShape.Points[idx].Add(spriteShape.Position.Point),
                    spriteShape.Points[idx + 1].Add(spriteShape.Position.Point),
                    new Location(rect1.BoundingRect.Left, rect1.BoundingRect.Top),
                    new Location(rect1.BoundingRect.Left + rect1.BoundingRect.Width, rect1.BoundingRect.Top),
                    ref intersection))
                    return true;

                if (FindIntersection(spriteShape.Points[idx].Add(spriteShape.Position.Point),
                    spriteShape.Points[idx + 1].Add(spriteShape.Position.Point),
                    new Location(rect1.BoundingRect.Left + rect1.BoundingRect.Width, rect1.BoundingRect.Top),
                    new Location(rect1.BoundingRect.Left + rect1.BoundingRect.Width, rect1.BoundingRect.Top + rect1.BoundingRect.Height),
                    ref intersection))
                    return true;

                if (FindIntersection(
                    spriteShape.Points[idx].Add(spriteShape.Position.Point),
                    spriteShape.Points[idx + 1].Add(spriteShape.Position.Point),
                    new Location(rect1.BoundingRect.Left + rect1.BoundingRect.Width, rect1.BoundingRect.Top + rect1.BoundingRect.Height),
                    new Location(rect1.BoundingRect.Left, rect1.BoundingRect.Top + rect1.BoundingRect.Height),
                    ref intersection))
                    return true;

                if (FindIntersection(
                    spriteShape.Points[idx].Add(spriteShape.Position.Point),
                    spriteShape.Points[idx + 1].Add(spriteShape.Position.Point),
                    new Location(rect1.BoundingRect.Left, rect1.BoundingRect.Top + rect1.BoundingRect.Height),
                    new Location(rect1.BoundingRect.Left, rect1.BoundingRect.Top),
                    ref intersection))
                    return true;
            }

            return false;
        }

        public static bool SphereToSphereCollisionDetection(ISprite sphere1, ISprite sphere2)
        {
            var delta = CollisionUtils.GetDistance(sphere1, sphere2);

            return (delta.Magnitude < (sphere1.Size.Width >> 1) + (sphere2.Size.Width >> 1));
        }

        public static int GetDistancePointToLine(PointF point, Location p1, Location p2)
        {
            int A = point.Point.X - p1.X;
            int B = point.Point.Y - p1.Y;
            int C = p2.X - p1.X;
            int D = p2.Y - p1.Y;

            return Math.Abs(A * D - C * B) / (int)Math.Sqrt(C * C + D * D);
        }

        public static bool SphereToLineListCollisionDetection(ISprite sphere1, ISprite lineList, ref Vector2 intersectionVector)
        {
            var spriteShape = lineList as ISpriteShape;

            if (spriteShape == null)
                throw new Exception("Invalid object type");

            var sphereRadius = sphere1.Size.Width >> 1;

            for (var idx = 0; idx < spriteShape.Points.Count - 1; ++idx)
            {
                int distance = GetDistancePointToLine(sphere1.Position, spriteShape.Points[idx], spriteShape.Points[idx + 1]);

                if (sphereRadius > distance)
                {
                    intersectionVector = new Vector2(spriteShape.Points[idx + 1].X - spriteShape.Points[idx].X,
                                           spriteShape.Points[idx + 1].Y - spriteShape.Points[idx].Y);
                    
                    return true;
                }
            }

            return false;
        }

        public static bool SphereListToSphereCollisionDetection(ISprite sphereList, ISprite sphere1)
        {
            return false;
        }

        public static bool SphereListToRectCollisionDetection(ISprite sphereList, ISprite rect)
        {
            return false;
        }

        public static bool SphereListToLineListCollisionDetection(ISprite sphereList, ISprite lineList)
        {
            return false;
        }

        public static bool SphereListToSphereListCollisionDetection(ISprite sphereList1, ISprite sphereList2)
        {
            return false;
        }

        public static bool LineListToLineListCollisionDetection(ISprite lineList1, ISprite lineList2)
        {
            return false;
        }

        public static bool Collided(ISprite sprite1, ISprite sprite2)
        {
            Vector2 intersectionVector = new Vector2();

            switch (sprite1.CollisionShape)
            {
                case CollisionShapes.Sphere:
                    switch (sprite2.CollisionShape)
                    {
                        case CollisionShapes.Sphere:
                            return SphereToSphereCollisionDetection(sprite1, sprite2);
                        case CollisionShapes.SphereList:
                            return SphereListToSphereCollisionDetection(sprite2, sprite1);
                        case CollisionShapes.Rectangle:
                            return RectToSphereCollisionDetection(sprite2, sprite1);
                        case CollisionShapes.LineList:
                            return SphereToLineListCollisionDetection(sprite1, sprite2, ref intersectionVector);
                    }
                    break;
                case CollisionShapes.SphereList:
                    switch (sprite2.CollisionShape)
                    {
                        case CollisionShapes.Sphere:
                            return SphereListToSphereCollisionDetection(sprite1, sprite2);
                        case CollisionShapes.SphereList:
                            return SphereListToSphereListCollisionDetection(sprite1, sprite2);
                        case CollisionShapes.Rectangle:
                            return SphereListToRectCollisionDetection(sprite1, sprite2);
                        case CollisionShapes.LineList:
                            return SphereListToLineListCollisionDetection(sprite1, sprite2);
                    }
                    break;
                case CollisionShapes.Rectangle:
                    switch (sprite2.CollisionShape)
                    {
                        case CollisionShapes.Sphere:
                            return RectToSphereCollisionDetection(sprite1, sprite2);
                        case CollisionShapes.SphereList:
                            return SphereListToRectCollisionDetection(sprite1, sprite2);
                        case CollisionShapes.Rectangle:
                            return RectToRectCollisionDetection(sprite1, sprite2);
                        case CollisionShapes.LineList:
                            return RectToLineListCollisionDetection(sprite1, sprite2, ref intersectionVector);
                    }
                    break;
                case CollisionShapes.LineList:
                    switch (sprite2.CollisionShape)
                    {
                        case CollisionShapes.Sphere:
                            return SphereToLineListCollisionDetection(sprite2, sprite1, ref intersectionVector);
                        case CollisionShapes.SphereList:
                            return SphereListToSphereListCollisionDetection(sprite1, sprite2);
                        case CollisionShapes.Rectangle:
                            return RectToLineListCollisionDetection(sprite2, sprite1, ref intersectionVector);
                        case CollisionShapes.LineList:
                            return LineListToLineListCollisionDetection(sprite1, sprite1);
                    }
                    break;
            }

            throw new InvalidOperationException("Unknown mapping.");
        }

        public static bool DetectCollision(ISprite sprite1, ISprite sprite2)
        {
            var collided = CollisionUtils.Collided(sprite1, sprite2);

            if (collided)
            {
                sprite1.OnCollide(new CollisionEventArgs(sprite2));
                sprite1.CollisionList.Add(sprite2);
                sprite2.OnCollide(new CollisionEventArgs(sprite1));
                sprite2.CollisionList.Add(sprite1);
            }

            return collided;
        }

        public static bool DetectSeperation(ISprite sprite1, ISprite sprite2)
        {
            var seperated = !CollisionUtils.Collided(sprite1, sprite2);

            if (seperated)
            {
                sprite1.OnSeperation(new CollisionEventArgs(sprite2));
                sprite1.CollisionList.Remove(sprite2);
                sprite2.OnSeperation(new CollisionEventArgs(sprite1));
                sprite2.CollisionList.Remove(sprite1);
            }

            return seperated;
        }

        public static void UpdateForcesAfterCollision(ISprite sprite1, ISprite sprite2)
        {
            var delta = CollisionUtils.GetDistance(sprite1, sprite2);

            double theta1 = delta.Direction;
            double theta2 = delta.Direction;

            var v1p = (sprite1.Force.X * Math.Cos(theta1) + sprite1.Force.Y * Math.Sin(theta1));
            var v1n = (-sprite1.Force.X * Math.Sin(theta1) + sprite1.Force.Y * Math.Cos(theta1));

            var v2p = (sprite2.Force.X * Math.Cos(theta2) + sprite2.Force.Y * Math.Sin(theta2));
            var v2n = (-sprite2.Force.X * Math.Sin(theta2) + sprite2.Force.Y * Math.Cos(theta2));

            var tmp = 1 / (sprite1.Mass + sprite2.Mass);

            var e1 = sprite1.CoefRestitution;
            var e2 = sprite2.CoefRestitution;
            var m1 = sprite1.Mass;
            var m2 = sprite2.Mass;

            var v1pPrime = ((m1 - e1 * m2) * tmp * v1p) + ((1.0 + e1) * m2 * v2p * tmp);
            var v2pPrime = (((1.0 + e1) * m1 * v1p * tmp) + ((m2 - e1 * m1) * v2p * tmp));

            float v1x = (float)(v1pPrime * Math.Cos(theta1) - v1n * Math.Sin(theta1));
            float v1y = (float)(v1pPrime * Math.Sin(theta1) + v1n * Math.Cos(theta1));

            float v2x = (float)(v2pPrime * Math.Cos(theta2) - v2n * Math.Sin(theta2));
            float v2y = (float)(v2pPrime * Math.Sin(theta2) + v2n * Math.Cos(theta2));     

            if (!sprite1.IsStatic)
                sprite1.Force = new Vector2(v1x, v1y);

            if (!sprite2.IsStatic)
                sprite2.Force = new Vector2(v2x, v2y);
        }

        public static bool FindIntersection(Location p1, Location p2, Location p3, Location p4, ref Vector2 intersection)
        {
            var rect1 = new System.Drawing.Rectangle(p1.X, p1.Y, p2.X, p2.Y);
            var rect2 = new System.Drawing.Rectangle(p3.X, p3.Y, p4.X, p4.Y);

            //If the two lines don't match, don't bother searching for 
            //intersection point.
            if (!rect1.IntersectsWith(rect2))
                return false;

            int xD1 = p2.X - p1.X;
            int xD2 = p4.X - p3.X;
            int yD1 = p2.Y - p1.Y;
            int yD2 = p4.Y - p3.Y;
            int xD3 = p1.X - p3.X;
            int yD3 = p1.Y - p3.Y;
            double dot, deg;
            double segmentLen1, segmentLen2;
            float ua, ub, div;
            // calculate differences

            // calculate the lengths of the two lines   
            double len1 = Math.Sqrt(xD1 * xD1 + yD1 * yD1);
            double len2 = Math.Sqrt(xD2 * xD2 + yD2 * yD2);

            // calculate angle between the two lines.  
            dot = (xD1 * xD2 + yD1 * yD2);

            // dot product  
            deg = dot / (len1 * len2);
            // if abs(angle)==1 then the lines are parallell,   
            // so no intersection is possible 

            if (Math.Abs(deg) == 1)
                return false;

            // find intersection Pt between two lines   
            div = yD2 * xD1 - xD2 * yD1;
            ua = (xD2 * yD3 - yD2 * xD3) / div;
            ub = (xD1 * yD3 - yD1 * xD3) / div;

            var point = new Location(Convert.ToInt32(p1.X + ua * xD1),Convert.ToInt32(p1.Y + ua * yD1));

            // calculate the combined length of the two segments 
            // between Pt-p1 and Pt-p2
            xD1 = point.X - p1.X;
            xD2 = point.X - p2.X;
            yD1 = point.Y - p1.Y;
            yD2 = point.Y - p2.Y;
            segmentLen1 = Math.Sqrt(xD1 * xD1 + yD1 * yD1) + Math.Sqrt(xD2 * xD2 + yD2 * yD2);

            // calculate the combined length of the two segments  
            // between Pt-p3 and Pt-p4
            xD1 = point.X - p3.X;
            xD2 = point.X - p4.X;
            yD1 = point.Y - p3.Y;
            yD2 = point.Y - p4.Y;
            segmentLen2 = Math.Sqrt(xD1 * xD1 + yD1 * yD1) + Math.Sqrt(xD2 * xD2 + yD2 * yD2);
            // if the lengths of both sets of segments are the same as  
            // the lenghts of the two lines the point is actually 
            // on the line segment.  
            // if the point isn’t on the line, return null   

            if (Math.Abs(len1 - segmentLen1) > 0.01 || Math.Abs(len2 - segmentLen2) > 0.01)
                return false;

            intersection = new Vector2(point.X, point.Y);

            return true;
        }
    }
}
